#include "xunji_task.h"
#include "FreeRTOS.h"
#include "task.h"

#include "uart.h"
#include <stdio.h>   // 用于 printf
#include <stdlib.h>  // 用于 atoi
#include "motor_task.h"
#include "control.h"
#include "change.h"


static uint8_t task1_flag;   //任务1检测到黑线停车的标记


static uint8_t task2_flag_1 = 0; //任务2
uint8_t task2_flag_2 = 0;
uint8_t xunji_target = 0,xunji_send_flag = 0;


/* 私有函数和变量（仅本文件可见） */
static TaskHandle_t xMotorTaskHandle = NULL;  // 任务句柄



uint8_t SevenXunJi_Read(void) {
    uint8_t state = 0;
    state |= (DL_GPIO_readPins(sevenxunji_n1_PORT, sevenxunji_n1_PIN)) ? 1 << 0 : 0; // n1
    state |= (DL_GPIO_readPins(sevenxunji_n2_PORT, sevenxunji_n2_PIN)) ? 1 << 1 : 0; // n2
    state |= (DL_GPIO_readPins(sevenxunji_n3_PORT, sevenxunji_n3_PIN)) ? 1 << 2 : 0; // n3
    state |= (DL_GPIO_readPins(sevenxunji_n4_PORT, sevenxunji_n4_PIN)) ? 1 << 3 : 0; // n4
    state |= (DL_GPIO_readPins(sevenxunji_n5_PORT, sevenxunji_n5_PIN)) ? 1 << 4 : 0; // n5
    state |= (DL_GPIO_readPins(sevenxunji_n6_PORT, sevenxunji_n6_PIN)) ? 1 << 5 : 0; // n6
    state |= (DL_GPIO_readPins(sevenxunji_n7_PORT, sevenxunji_n7_PIN)) ? 1 << 6 : 0; // n7
    return state; // 例如 0b0001000 表示中间传感器检测到黑线
}


//主机右轮函数
/* 任务主函数 */
#ifdef MASTER_MODE
void xunjiTask(void *pvParameters) 
{
  
    /* 任务主循环 */
    for (;;) {
      uint8_t xunji_return;
      xunji_return = SevenXunJi_Read();
    //   printf("%d\r\n",xunji_return);
    //   if(task_mode == 0)
    //   {
    //     xunji_target = 0;
    //     if(xunji_return != 0)
    //     {
    //         task1_flag++;
    //     }
    //     if(task1_flag >30)
    //     {
    //         xunji_target = 30;
    //     }
    //   }  //任务1

    //   else if(task_mode == 1)
    //   {
    //     if(xunji_return != 0 && task2_flag_1 == 0)
    //     {
    //         task1_flag++;
    //     }
    //     if(task1_flag >30 && task2_flag_1 == 0)
    //     {
    //         task2_flag_1 = 1;  //第一次入线
    //         task1_flag = 0;
    //     }

    //     if(task2_flag_1 == 1 && xunji_return == 0)
    //     {
    //         task1_flag++;
    //     }
    //     if(task1_flag >30)
    //     {
    //         task2_flag_1 = 2;  //第一次出线
    //         task1_flag = 0;
    //         xunji_target = 30;
    //         task2_flag_2 = 1;
    //     }

        // if(xunji_return != 0 && task2_flag_1 == 2)
        // {
        //     task1_flag++;
        // }
        // if(task1_flag >30 && task2_flag_1 == 2)
        // {
        //     task2_flag_1 = 3;  //第2次入线
        //     task1_flag = 0;
        //     task2_flag_2 = 2;
        // }


        // if(task2_flag_1 == 3 && xunji_return == 0)
        // {
        //     task1_flag++;
        // }
        // if(task1_flag >30)
        // {
        //     task2_flag_1 = 4;  //第2次出线
        //     task1_flag = 0;
        //     xunji_target = 30;
        //     task2_flag_2 = 3;
        // }
        // if(task2_flag_1 != 2)
        // {
            switch (xunji_return) {
                case 1:     //000 0 001
                    xunji_target = 30;   //   最左边黑线,发送给从机，从机加速  18不够，18可以循迹圆圈
                    xunji_send_flag = 1;
                    break;
                case 3:     //000 0 011
                    xunji_target = 30;
                    xunji_send_flag = 1;
                    break;
                case 2:     //000 0 010
                    xunji_target = 30;
                    xunji_send_flag = 1;
                    break;
                case 6:     //000 0 110
                    xunji_target = 30;
                    xunji_send_flag = 1;
                    break;
                
                case 4:     //000 0 100
                    xunji_target = 20;
                    xunji_send_flag = 1;
                    break;
                case 12:    //000 1 100
                    xunji_target = 3;
                    xunji_send_flag = 1;
                    break;

                case 15:      //000 1 111           000 0 111      //直角弯
                case  7:
                    xunji_target = 60;
                    xunji_send_flag = 1;
                    vTaskDelay(pdMS_TO_TICKS(252)); 
                    break;
        /******************************************************************************** */
                case 0:
                case 8:     //000 1 000
                    xunji_target = 0;
                    xunji_send_flag = 1;
                    break;
        /******************************************************************************** */
                case 120:     //111 1 000          111 0 000     //直角弯
                case 112:
                    xunji_target = 60;
                    xunji_send_flag = 0;
                    vTaskDelay(pdMS_TO_TICKS(252));   //500转180了
                    break;

                case 24:    //001 1 000   //主机加速
                    xunji_target = 3;
                    xunji_send_flag = 0;
                    break;

                case 16:    //001 0 000
                    xunji_target = 20;
                    xunji_send_flag = 0;
                    break;
                case 48:    //011 0 000
                    xunji_target = 30;
                    xunji_send_flag = 0;
                    break;

                case 32:    //010 0 000
                    xunji_target = 30;
                    xunji_send_flag = 0;
                    break;

                case 96:    //110 0 000
                    xunji_target = 30;
                    xunji_send_flag = 0;
                    break;

                case 64:    //100 0 000
                    xunji_target = 30;
                    xunji_send_flag = 0;
                    break;
                
                default:
                    xunji_target = 0;
                    xunji_send_flag = 1;
                    break;
            
            }
        // }
    //   }
    //   printf("%d\r\n",xunji_target);
        // UART_SendByte(xunji_target);
        /* 延迟1ms（根据需求调整） */
        vTaskDelay(pdMS_TO_TICKS(1));
    }

    /* 理论上不会执行到这里 */
    vTaskDelete(NULL);
}
#else
void xunjiTask(void *pvParameters) 
{
  
    /* 任务主循环 */
    for (;;) {

    //   printf("%d\r\n",xunji_target);
        // UART_SendByte(xunji_target);
        /* 延迟1ms（根据需求调整） */
        vTaskDelay(pdMS_TO_TICKS(1));
    }

    /* 理论上不会执行到这里 */
    vTaskDelete(NULL);
}

#endif